{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# AHRSFactor\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/AHRSFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
        "\n",
        "## Overview\n",
        "\n",
        "The `AHRSFactor` class implements a factor to implement an *Attitude and Heading Reference System* (AHRS) within GTSAM. It is a binary factor, taking preintegrated measurements from a gyroscope between two discrete time steps, typically denoted as $t_i$ and $t_j$. These preintegrated measurements encapsulate the rotational motion observed by an inertial measurement unit (IMU) between these two timestamps.\n",
        "\n",
        "The `AHRSFactor` thus constrains two attitude states (represented as elements of $SO(3)$) based solely on gyroscope measurements. Accelerometer or magnetometer aiding, needed to build a complete AHRS system, must be handled separately."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Note: you may need to restart the kernel to use updated packages.\n"
          ]
        }
      ],
      "source": [
        "%pip install --quiet gtsam-develop plotly"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Mathematical Formulation\n",
        "\n",
        "The `AHRSFactor` relies on the use of `PreintegratedRotation`, which applies successive exponential maps to each individual gyroscope measurement $\\omega$ over the interval $[t_i, t_j]$. In this approach, every measurement contributes a small rotation given by $\\exp((\\omega_k - \\hat{b}_g) \\Delta t)$, where $\\hat{b}_g$ is an initial estimate of the gyroscope bias. The overall preintegrated rotation is obtained by composing these incremental rotations:\n",
        "$$\n",
        "\\Delta R_{ij} = \\prod_{k} \\exp((\\omega_k - \\hat{b}_g) \\Delta t)\n",
        "$$\n",
        "\n",
        "Given two estimated rotations $R_i$ at time $t_i$ and $R_j$ at time $t_j$, and a current estimate of the gyroscope bias $b_g$, the factor predicts the rotation at time $j$ as:\n",
        "$$\n",
        "\\hat{R}_j = R_i \\cdot \\Delta \\tilde{R}_{ij}(b_g)\n",
        "$$\n",
        "where $\\Delta \\tilde{R}_{ij}(b_g)$ is the preintegrated measurement corrected for the new bias estimate $b_g$.\n",
        "\n",
        "The error term optimized by the factor graph is the rotational discrepancy captured by the logarithmic map:\n",
        "$$\n",
        "e = \\text{Log}\\left(\\hat{R}_j^T R_j\\right) = \\text{Log}\\left((\\Delta \\tilde{R}_{ij}(b_g))^T R_i^T R_j\\right)\n",
        "$$\n",
        "\n",
        "Note: the reality is a bit more complicated, because the code also takes into account the effects of bias correction Jacobians, and if desired, coriolis forces."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Key Functionality\n",
        "\n",
        "### Constructor\n",
        "\n",
        "The constructor initializes the factor using a preintegrated AHRS measurement object, relating orientation states at discrete time instances $t_i$ and $t_j$.\n",
        "\n",
        "### Core Methods\n",
        "\n",
        "- `evaluateError`: calculates the error between two estimated orientations at times $t_i$ and $t_j$, relative to the preintegrated gyroscopic measurements. The error is computed as:\n",
        "\n",
        "  $$\n",
        "  \\text{error} = \\text{Log}\\left((\\Delta R_{ij}^{meas})^T \\cdot R_i^T R_j\\right)\n",
        "  $$\n",
        "\n",
        "  Here:\n",
        "\n",
        "  - $R_i, R_j$ are the estimated rotation matrices at times $t_i$ and $t_j$.\n",
        "  - $\\Delta R_{ij}^{meas}$ is the rotation matrix obtained by integrating gyroscope measurements from $t_i$ to $t_j$.\n",
        "  - $\\text{Log}(\\cdot)$ is the logarithmic map from $SO(3)$ to $\\mathbb{R}^3$.\n",
        "\n",
        "  The resulting 3-dimensional error vector represents the rotational discrepancy.\n",
        "\n",
        "- `print`: outputs a readable representation of the internal state of the factor, including the associated time steps and preintegrated measurements, aiding debugging and verification.\n",
        "\n",
        "- `equals` determines if another `AHRSFactor` instance is identical, useful in testing scenarios or when verifying the correctness of factor graph constructions."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Usage\n",
        "\n",
        "First, create a PreintegratedAhrsMeasurements (PAM) object by supplying the necessary IMU parameters. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "from gtsam import PreintegrationParams, PreintegratedAhrsMeasurements\n",
        "\n",
        "params = PreintegrationParams.MakeSharedU(9.81)\n",
        "params.setGyroscopeCovariance(np.deg2rad(1)*np.eye(3))\n",
        "params.setAccelerometerCovariance(0.01*np.eye(3))\n",
        "\n",
        "biasHat = np.zeros(3)  # Assuming no bias for simplicity\n",
        "pam = PreintegratedAhrsMeasurements(params, biasHat)  "
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "\n",
        "Next, integrate each gyroscope measurement along with its corresponding time interval to accumulate the preintegrated rotation."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {},
      "outputs": [],
      "source": [
        "from gtsam import Point3\n",
        "np.random.seed(42)  # For reproducibility\n",
        "for _ in range(15):  # Add 15 random measurements, biased to move around z-axis\n",
        "    omega = Point3(0,0,-0.5) + 0.1*np.random.randn(3)  # Random angular velocity vector\n",
        "    pam.integrateMeasurement(omega, deltaT=0.1)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "Finally, construct the AHRSFactor using the accumulated PAM and the keys representing the rotation states at the two time instants."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {},
      "outputs": [],
      "source": [
        "from gtsam import AHRSFactor\n",
        "bias_key = 0  # Key for the bias\n",
        "i, j = 1, 2  # Indices of the two attitude unknowns\n",
        "ahrs_factor = AHRSFactor(i, j, bias_key, pam)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "AHRSFactor(1,2,0,  preintegrated measurements:    deltaTij [1.5]\n",
            "    deltaRij.ypr = (  -0.82321 -0.0142842  0.0228577)\n",
            "biasHat [0 0 0]\n",
            " PreintMeasCov [   0.0261799 1.73472e-18 1.35525e-20\n",
            "1.73472e-18   0.0261799 9.23266e-20\n",
            "1.35525e-20 1.17738e-19   0.0261799 ]\n",
            "  noise model: diagonal sigmas [0.161802159; 0.161802159; 0.161802159];\n",
            "\n"
          ]
        }
      ],
      "source": [
        "print(ahrs_factor)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Source\n",
        "- [AHRSFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.h)\n",
        "- [AHRSFactor.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/AHRSFactor.cpp)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 2
}